#!/usr/bin/env python
# -*- coding: utf-8 -*-
# @Time    : 2024/1/30 17:13
# @Author  : xinyingjie
# @File    : tools.py
# @Description :
from datetime import datetime
from math import *
import time
from collections import defaultdict
import random
from typing import List, Dict, Tuple
from loguru import logger
import math

from datetime import datetime

from utils.config_temp import RADAR_DATA_LIST_CJ as RADAR_DATA_LIST


def time_str(timestamp_in_milliseconds):
    # 转换为datetime对象
    date_time_obj = datetime.fromtimestamp(timestamp_in_milliseconds / 1000)
    # 格式化输出为你想要的格式
    formatted_time = date_time_obj.strftime('%Y-%m-%d %H:%M:%S.%f')[:-3]

    return formatted_time


def get_next_target_car_info_jingtai(t_start, target_next_id, longitude, latitude, per_car_info_dict, match_info_dict,
                                     speed, vehicle_id):
    min_dis = math.inf
    target_car_info = None
    dis_other = -1
    for car_id in list(per_car_info_dict[target_next_id].keys()):
        if car_id in match_info_dict[target_next_id].keys():
            continue
        t_info = per_car_info_dict[target_next_id][car_id]
        other_last_time = max(list(t_info.keys()))
        other_last_info = per_car_info_dict[target_next_id][car_id][other_last_time]
        # logger.info(f"仿真车{vehicle_id} 可匹配的感知车为{per_car_info_dict[target_next_id].keys()}")
        if (t_start - other_last_time) < 500:
            other_id = other_last_info['id']
            simu_is_front = is_in_front_jingtai(longitude, latitude, other_last_info['lon'],
                                                other_last_info['lat'], other_last_info['angle'])

            if not simu_is_front:
                dis_target = calculate_distance(longitude, latitude, other_last_info['lon'],
                                                other_last_info['lat'])

                dis_other = dis_target - speed * 2
                # if dis_other < 150 and etc_info['type'] == other_last_info['type']:
                if -10 < dis_other < 30:
                    if dis_other < min_dis:
                        target_car_info = other_last_info
                        min_dis = dis_other
    # logger.info(f"车辆{etc_info['id']}   配置的感知车辆为{target_car_info['id'] if target_car_info else '未匹配'} 距离为{min_dis}")
    # logger.info(f"车辆{other_last_info['id']}   距离为{dis_other}")
    # logger.info(f"仿真车车型{etc_info['type']}  感知车车型{other_last_info['type']}")
    return target_car_info, min_dis, min_dis + speed * 2


def get_next_target_car_info_temp(t_start, target_next_id, longitude, latitude, per_car_info_dict):
    min_dis = math.inf
    target_car_info = None
    dis_other = -1
    for car_id in list(per_car_info_dict[target_next_id].keys()):

        t_info = per_car_info_dict[target_next_id][car_id]
        other_last_time = max(list(t_info.keys()))
        other_last_info = per_car_info_dict[target_next_id][car_id][other_last_time]

        if (t_start - other_last_time) < 1000:
            other_id = other_last_info['id']
            dis_other = calculate_distance(longitude, latitude, other_last_info['longitude'],
                                           other_last_info['latitude'])

            # if dis_other < 150 and etc_info['type'] == other_last_info['type']:
            if dis_other < 100:
                if dis_other < min_dis:
                    target_car_info = other_last_info
                    min_dis = dis_other
    # logger.info(f"车辆{etc_info['id']}   配置的感知车辆为{target_car_info['id'] if target_car_info else '未匹配'} 距离为{min_dis}")
    # logger.info(f"车辆{other_last_info['id']}   距离为{dis_other}")
    # logger.info(f"仿真车车型{etc_info['type']}  感知车车型{other_last_info['type']}")
    return target_car_info, dis_other


def get_next_target_car_info(t_start, target_next_id, longitude, latitude, etc_info, per_car_info_dict):
    min_dis = math.inf
    target_car_info = None
    for car_id in list(per_car_info_dict[target_next_id].keys()):
        t_info = per_car_info_dict[target_next_id][car_id]
        other_last_time = max(list(t_info.keys()))
        other_last_info = per_car_info_dict[target_next_id][car_id][other_last_time]

        if (t_start - other_last_time) < 2000:
            other_id = other_last_info['id']
            dis_other = calculate_distance(longitude, latitude, other_last_info['lon'],
                                           other_last_info['lat'])

            # if dis_other < 150 and etc_info['type'] == other_last_info['type']:
            if dis_other < 150:
                if dis_other < min_dis:
                    target_car_info = other_last_info
                    min_dis = dis_other
    # logger.info(f"车辆{etc_info['id']}   配置的感知车辆为{target_car_info['id'] if target_car_info else '未匹配'} 距离为{min_dis}")
    # logger.info(f"车辆{other_last_info['id']}   距离为{dis_other}")
    # logger.info(f"仿真车车型{etc_info['type']}  感知车车型{other_last_info['type']}")
    return target_car_info


def is_in_front(car_simu, car_radar):
    """
       判断 仿真车 是否在 感知车 的前方
       参数:
           car_simu, car_radar: 字典，包含 'lon'、'lat' 和 'ang'（航向角，0-360度）
       返回:
           True: 如果 car_simu 在 car_radar 的前方
           False: 如果 car_simu 在 car_radar 的后方
    """
    # simu_lon = car_simu['longitude']
    # simu_lat = car_simu['latitude']
    # radar_lon = car_radar['longitude']
    # radar_lat = car_radar['latitude']

    # 计算从 car2 到 car1 的方位角（考虑地球曲率）
    # print(car_simu,car_radar)
    bearing = get_angle(
        car_radar['lon'], car_radar['lat'],
        car_simu['lon'], car_simu['lat']
    )

    # 获取 car2 的航向角
    car2_heading = car_radar['ang']

    # 计算相对角度差（目标方位与当前航向的差值）
    # 将差值标准化到 [-180, 180] 度
    angle_diff = (bearing - car2_heading + 180) % 360 - 180

    # 如果角度差在 -90 到 90 度之间，则 car1 在 car2 的前方
    return -90 <= angle_diff <= 90


def is_in_front_jingtai(simu_lon, simu_lat, radar_lon, radar_lat, radar_angle):
    """
       判断 仿真车 是否在 感知车 的前方
       参数:
           car_simu, car_radar: 字典，包含 'lon'、'lat' 和 'ang'（航向角，0-360度）
       返回:
           True: 如果 car_simu 在 car_radar 的前方
           False: 如果 car_simu 在 car_radar 的后方
    """
    # simu_lon = car_simu['longitude']
    # simu_lat = car_simu['latitude']
    # radar_lon = car_radar['longitude']
    # radar_lat = car_radar['latitude']

    # 计算从 car2 到 car1 的方位角（考虑地球曲率）
    bearing = get_angle(
        radar_lon, radar_lat,
        simu_lon, simu_lat
    )

    # 获取 car2 的航向角
    car2_heading = radar_angle

    # 计算相对角度差（目标方位与当前航向的差值）
    # 将差值标准化到 [-180, 180] 度
    angle_diff = (bearing - car2_heading + 180) % 360 - 180

    # 如果角度差在 -90 到 90 度之间，则 car1 在 car2 的前方
    return -90 <= angle_diff <= 90


############################################## 点B基于点A的方向，以北为基准  ############################################
def get_angle(lonA, latA, lonB, latB):
    """
    参数:
        点A (lonA, latA)
        点B (lonB, latB)
    返回:
        点B基于点A的方向，以北为基准,顺时针方向的角度，0~360度
    """
    radLatA = radians(latA)
    radLonA = radians(lonA)
    radLatB = radians(latB)
    radLonB = radians(lonB)
    dLon = radLonB - radLonA
    y = sin(dLon) * cos(radLatB)
    x = cos(radLatA) * sin(radLatB) - sin(radLatA) * cos(radLatB) * cos(dLon)
    angle = degrees(atan2(y, x))
    angle = (angle + 360) % 360
    return angle


def get_timestamp_milliseconds(t_str):
    """
    将ISO 8601时间戳字符串转为13位毫秒级时间戳
    :param t_str: ISO 8601时间戳字符串
    :return: 13位毫秒级时间戳
    """
    # t_str = data['timestamp']
    # 解析 ISO 8601 时间字符串为 datetime 对象
    dt = datetime.strptime(t_str, "%Y-%m-%d %H:%M:%S.%f")
    # 将 datetime 对象转换为秒级时间戳
    timestamp_seconds = dt.timestamp()
    # 将秒级时间戳转换为毫秒级时间戳
    timestamp_milliseconds = int(timestamp_seconds * 1000)
    return timestamp_milliseconds


def get_timestamp_milliseconds_jingtai(t_str):
    """
    将ISO 8601时间戳字符串转为13位毫秒级时间戳
    :param t_str: ISO 8601时间戳字符串
    :return: 13位毫秒级时间戳
    """
    # t_str = data['timestamp']
    # 解析 ISO 8601 时间字符串为 datetime 对象
    dt = datetime.strptime(t_str, "%Y-%m-%d %H:%M:%S:%f")
    # 将 datetime 对象转换为秒级时间戳
    timestamp_seconds = dt.timestamp()
    # 将秒级时间戳转换为毫秒级时间戳
    timestamp_milliseconds = int(timestamp_seconds * 1000)
    return timestamp_milliseconds


def find_vehicles_by_duration_optimized(base_station_infos, min_duration_sec, max_disappear_sec, max_delete_sec):
    """
    高效找出持续出现或消失超过指定秒数的车辆

    Args:
        base_station_infos: defaultdict(dict), 基站数据，格式为 {station_id: {vehicle_id: {timestamp: info}}}
        min_duration_sec: float, 持续出现的最小秒数阈值
        max_disappear_sec: float, 消失的最大秒数阈值
        max_delete_sec: float, 缓存删除的最大秒数阈值
    Returns:
        tuple: (continuous_vehicles, disappeared_vehicles)
            - continuous_vehicles: {station_id: [(vehicle_id, duration_sec, first_seen, last_seen)]}
            - disappeared_vehicles: {station_id: [(vehicle_id, disappear_sec, last_seen)]}
    """
    continuous_vehicles = defaultdict(dict)
    disappeared_vehicles = defaultdict(list)
    delete_vehicles = defaultdict()
    current_time = int(time.time() * 1000)  # 只计算一次当前时间

    for station_id, vehicles in base_station_infos.items():
        for vehicle_id, timestamps_info in vehicles.items():
            if not timestamps_info:
                continue  # 跳过无数据的车辆

            # 获取排序后的时间戳（假设时间戳可能无序）
            timestamps = sorted(timestamps_info.keys())
            first_seen = timestamps[0]
            last_seen = timestamps[-1]
            last_info = timestamps_info[last_seen]
            first_info = timestamps_info[first_seen]

            # 检查持续出现时间
            duration = last_seen - first_seen
            if duration >= min_duration_sec:
                continuous_vehicles[station_id][vehicle_id] = (vehicle_id, duration, first_seen, last_seen)

            # 检查消失时间
            disappear_time = current_time - last_seen
            # if disappear_time >= max_disappear_sec and vehicle_id in continuous_vehicles[station_id].keys():
            if RADAR_DATA_LIST[station_id]['type'] == 'jg':
                target_max_disappear_sec = max_disappear_sec
            else:
                target_max_disappear_sec = max_disappear_sec + 1200
            if disappear_time >= target_max_disappear_sec and duration >= min_duration_sec:
                # print(last_seen,first_seen,duration)
                # dis = calculate_distance(first_info['longitude'],first_info['latitude'],last_info['longitude'],last_info['latitude'])
                # speed = dis / (duration / 1000)
                # print(f"计算速度{speed}  感知速度{last_info['speed'] / 100}")
                disappeared_vehicles[station_id].append(
                    (vehicle_id, disappear_time, last_seen, last_info)
                )

            if disappear_time >= max_delete_sec:
                # logger.info(f"删除单个车辆{vehicle_id}")
                delete_vehicles[vehicle_id] = last_info['sourceId']

    return continuous_vehicles, disappeared_vehicles, delete_vehicles


def calculate_vehicle_acceleration_deceleration(
        all_vehicle_ids: List[str],
        current_accelerating: List[str],
        current_decelerating: List[str],
        accelerate_percent: float,
        decelerate_percent: float,
        duration_range: Tuple[int, int] = (10, 60)  # 默认持续时间改为10-60秒
) -> List[Dict[str, any]]:
    """
    计算车辆加速和减速状态

    参数:
        all_vehicle_ids: 所有车辆ID列表
        current_accelerating: 当前正在加速的车辆ID列表
        current_decelerating: 当前正在减速的车辆ID列表
        accelerate_percent: 需要加速的车辆百分比(0-1)
        decelerate_percent: 需要减速的车辆百分比(0-1)
        duration_range: 持续时间范围(秒)，现在默认为10-60秒

    返回:
        包含车辆ID、状态和持续时间的字典列表
    """
    # 1. 获取当前未处于加速或减速状态的车辆
    active_vehicles = set(all_vehicle_ids)
    inactive_vehicles = list(active_vehicles - set(current_accelerating) - set(current_decelerating))

    # 2. 计算需要加速和减速的车辆数量
    total_vehicles = len(inactive_vehicles)
    accelerate_count = int(total_vehicles * accelerate_percent)
    decelerate_count = int(total_vehicles * decelerate_percent)

    # 确保不超过可用车辆数
    accelerate_count = min(accelerate_count, len(inactive_vehicles))
    decelerate_count = min(decelerate_count, len(inactive_vehicles) - accelerate_count)

    # 3. 随机选择车辆
    random.shuffle(inactive_vehicles)

    # 分配加速车辆
    accelerating = inactive_vehicles[:accelerate_count]
    remaining_vehicles = inactive_vehicles[accelerate_count:]

    # 分配减速车辆
    decelerating = remaining_vehicles[:decelerate_count]

    # 4. 生成结果
    result = []

    # 添加加速车辆
    for vehicle_id in accelerating:
        duration = random.randint(*duration_range)  # 随机10-60秒
        result.append({
            'id': vehicle_id,
            'action': 'accelerate',
            'duration': duration
        })

    # 添加减速车辆
    for vehicle_id in decelerating:
        duration = random.randint(*duration_range)  # 随机10-60秒
        result.append({
            'id': vehicle_id,
            'action': 'decelerate',
            'duration': duration
        })

    return result


def calculate_distance(jing1, wei1, jing2, wei2):
    lng1, lat1, lng2, lat2 = map(radians, [jing1, wei1, jing2, wei2])  # 经纬度转换成弧度
    dlon = lng2 - lng1
    dlat = lat2 - lat1
    a = sin(dlat / 2) ** 2 + cos(lat1) * cos(lat2) * sin(dlon / 2) ** 2
    distance = 2 * asin(sqrt(a)) * 6371.393 * 1000  # 地球平均半径，6371.393km

    return distance
